#include "vehicle_operation_dispatcher.h"

namespace application {
namespace vehicle {

void VehicleOperationDispatcher::registerCommandImplementation(
    int agvNumber,
    std::shared_ptr<kernel::agv::IAgvCommand> commandImpl)
{
    m_commandMap[agvNumber] = commandImpl;
}

void VehicleOperationDispatcher::unregisterCommandImplementation(int agvNumber)
{
    m_commandMap.erase(agvNumber);
}

kernel::Result<void> VehicleOperationDispatcher::moveToStationList(
    kernel::agv::AgvEntity& agv,
    const std::vector<std::string>& stations,
    const std::string& operation)
{
    auto commandImpl = getCommandImplementation(agv.getNumber());
    if (!commandImpl) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "AGV command implementation not found"
        );
    }
    return commandImpl->moveToStationList(agv.getNumber(), stations, operation);
}

kernel::Result<void> VehicleOperationDispatcher::moveToStation(
    kernel::agv::AgvEntity& agv,
    const std::string& sourceId,
    const std::string& targetId)
{
    auto commandImpl = getCommandImplementation(agv.getNumber());
    if (!commandImpl) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "AGV command implementation not found"
        );
    }
    return commandImpl->moveToStation(agv.getNumber(), sourceId, targetId);
}

kernel::Result<void> VehicleOperationDispatcher::cancelTask(
    kernel::agv::AgvEntity& agv)
{
    auto commandImpl = getCommandImplementation(agv.getNumber());
    if (!commandImpl) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "AGV command implementation not found"
        );
    }
    return commandImpl->cancelTask(agv.getNumber());
}

kernel::Result<void> VehicleOperationDispatcher::pauseTask(
    kernel::agv::AgvEntity& agv)
{
    auto commandImpl = getCommandImplementation(agv.getNumber());
    if (!commandImpl) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "AGV command implementation not found"
        );
    }
    return commandImpl->pauseTask(agv.getNumber());
}

kernel::Result<void> VehicleOperationDispatcher::resumeTask(
    kernel::agv::AgvEntity& agv)
{
    auto commandImpl = getCommandImplementation(agv.getNumber());
    if (!commandImpl) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "AGV command implementation not found"
        );
    }
    return commandImpl->resumeTask(agv.getNumber());
}

kernel::Result<void> VehicleOperationDispatcher::controlJack(
    kernel::agv::AgvEntity& agv,
    bool raise)
{
    auto commandImpl = getCommandImplementation(agv.getNumber());
    if (!commandImpl) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "AGV command implementation not found"
        );
    }
    return commandImpl->controlJack(agv.getNumber(), raise);
}

kernel::Result<void> VehicleOperationDispatcher::startCharging(
    kernel::agv::AgvEntity& agv,
    const std::string& chargePoint)
{
    auto commandImpl = getCommandImplementation(agv.getNumber());
    if (!commandImpl) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "AGV command implementation not found"
        );
    }
    return commandImpl->startCharging(agv.getNumber(), chargePoint);
}

kernel::Result<void> VehicleOperationDispatcher::stopCharging(
    kernel::agv::AgvEntity& agv)
{
    auto commandImpl = getCommandImplementation(agv.getNumber());
    if (!commandImpl) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "AGV command implementation not found"
        );
    }
    return commandImpl->stopCharging(agv.getNumber());
}

kernel::Result<void> VehicleOperationDispatcher::controlDoor(
    kernel::agv::AgvEntity& agv,
    int doorId,
    bool open)
{
    auto commandImpl = getCommandImplementation(agv.getNumber());
    if (!commandImpl) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "AGV command implementation not found"
        );
    }
    return commandImpl->controlDoor(agv.getNumber(), doorId, open);
}

kernel::Result<void> VehicleOperationDispatcher::switchMap(
    kernel::agv::AgvEntity& agv,
    const std::string& mapName,
    const std::string& switchPoint)
{
    auto commandImpl = getCommandImplementation(agv.getNumber());
    if (!commandImpl) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "AGV command implementation not found"
        );
    }
    return commandImpl->switchMap(agv.getNumber(), mapName, switchPoint);
}

kernel::Result<void> VehicleOperationDispatcher::requestCurrentMap(
    kernel::agv::AgvEntity& agv)
{
    auto commandImpl = getCommandImplementation(agv.getNumber());
    if (!commandImpl) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "AGV command implementation not found"
        );
    }
    return commandImpl->requestCurrentMap(agv.getNumber());
}

bool VehicleOperationDispatcher::hasCommandImplementation(int agvNumber) const
{
    return m_commandMap.find(agvNumber) != m_commandMap.end();
}

size_t VehicleOperationDispatcher::getRegisteredVehicleCount() const
{
    return m_commandMap.size();
}

std::shared_ptr<kernel::agv::IAgvCommand> VehicleOperationDispatcher::getCommandImplementation(
    int agvNumber) const
{
    auto it = m_commandMap.find(agvNumber);
    if (it != m_commandMap.end()) {
        return it->second;
    }
    return nullptr;
}

} // namespace vehicle
} // namespace application
